/*****************************************************************************
 * BasicServo_v2.c
 *
 * To flash the bootloader:
 * 1) Project > Project Options > Type: Loader File
 * 2) Rebuild Project
 * 3) Tools > Flash Programmer.. > Browse > 
 *        ..\ADSP-21369 EZ-KIT Lite\Flash Programmer\Parallel\21369EzFlashDriver_Parallel.dxe
 * 4) Hit Load
 * 5) Click programming tab and find the linker file. Click 'Program'
 * 6) Program will now boot on power-up
 *
 *****************************************************************************/

 
#include <stdio.h>
#include <def21369.h>
#include <cdef21369.h>
#include <21369.h>
#include <math.h>
#include <signal.h>

//#define SRUDEBUG  // Check SRU Routings for errors.
//#include <SRU.h>
//#include "tt.h"
#define SIZE		11
#define ON			1
#define OFF			0

/////////////////////////////
/*   Function Prototypes   */
/////////////////////////////

//Externally defined
extern void SetupUART(void);	//function for initializing the UART
extern void sendData(long); //routine for sending data over UART
extern void initPLL_SDRAM(void);    //Initialize PLL to 393 MHz and set up AMI and SDRAM controller
extern void SetupADC(void);
extern void SetupDAC(void);
extern void SetCS(int);
extern void SetupGPIO(void);
extern void InitConversion(void);
extern long ReadSPI(void);
extern void WriteSPI(unsigned long);
extern void delay(int);
extern void IVSetup(void);
extern void DoIV(float,int);
extern void SendIV(void);
extern void ResumeTunneling(void);

//Internally defined
float ADCtoVolts(long);
unsigned long VoltstoDAC(float);
float PID(float,float);
void StepResponse(bool);
void timer_isr(int);
void StopServo(void);
void StartServo(void);
void Hold(bool);
void Retract(bool);
void Reset(void);
void ReportSettings(float,int);
void TipVolts(void);
void SendFlt(float);
float ReceiveFloat(void);
void SendState(void);
void SetCurrentV(int);
void SwitchChannel(int);
void SwitchGain(int);
void SetHG(void);
void SetLG(void);
void SwapV(void);
void ServiceLGFlag(void);
void CurrentSetpoint(void);
void SetGain(int);
float MeasureOffset(void);
void SendZ(void);
void ChangeZLG(void);
void SetAlpha(void);

/* Integer Conversion Parameters */
const float ADCRes = 2.9802322387695312e-7; // 5V/2^24
const float DACRes = 13107.2; // 2^18/20V
const float halfDACRes = 131072.0; // 2^17

volatile bool HOLD = OFF;
volatile bool RETRACT = OFF;
volatile bool GAIN = ON;
volatile bool flagLG = OFF;
volatile int flagIndex = 0;
volatile int zerone = 0;

const unsigned int HBIT = 1;
const unsigned int RBIT = 0;
const unsigned int GBIT = 2;

const int Z_CH_H =		0;	// The high gain z channel
const int Z_CH_L =		1;	// The high gain z channel
const int TIP_CH =		2;	// Tip channel

/* Initial PID Parameters */
float setpoint = 0.3; // the current setpoint in Volts
float lsetpoint;
float setstepmax = 0.75;
float lsetstepmax;
const int steplen = 1000;
volatile int index = 0;
volatile float findex = 0.0;
float Kp = 4e-4;
float Ki = 4e-4;
float Kd = 0.0;
float offset = 0.0;
float maxoff = 0.5;
float zmax = 9.5;
float errmax = 5.0;
int Tpid = 6635; // sets the servo freq
int ireads = 3; // # of reads for the tunneling current
float alpha = 0.5;

volatile float zerr = 0;
volatile float lsterr = 0;
volatile float lstz = 0;
volatile float integral = 0;
volatile float zvolts = 0;
volatile unsigned int zint = 0;
volatile long val = 0;
volatile float fval = 0.0;
volatile float pv_0 = 0.0;
volatile float pv_1 = 0.0;
volatile float pv_2 = 0.0;
//volatile float pvmin = 0.015;
volatile float Ti = 0.0;
volatile float Td = 0.0;
float DeadT = 25e-6;
float Ts = 0.0;


struct Vstore{
	float vHG;
	float vLG;
	float vTip;
	float vI;
	float *current;
	float vAcc;
	int chGain;
};

struct Vstore currentV;

typedef union byte2flt{ 
	float flt; 
	struct
    {
        int BYTE3 :8;
        int BYTE2 :8;
        int BYTE1 :8;
        int BYTE0 :8;
    } BYTE;
} BYTE2FLT;


//////////////////////////
/*   Define Functions   */
//////////////////////////
float MeasureOffset()
{
	int i;
	float tempoff;
	long intoff = 0;
	for (i=0;i<256;i++)
	{
		delay(Tpid);
		// Read the DAC //
		intoff += -1*ReadSPI(); // multiply by -1 to correct sign error
	}
	intoff = (intoff >> 8);
	tempoff = ADCtoVolts(intoff);
	if (tempoff > maxoff) tempoff = 0.0;
	else if (tempoff < -maxoff) tempoff = 0.0;
	return tempoff;
}
	

void SetCurrentV(int channel)
{
	if (channel == Z_CH_H) currentV.current = &currentV.vHG;
	else if (channel == Z_CH_L) currentV.current = &currentV.vLG;
	else if (channel == TIP_CH) currentV.current = &currentV.vTip;
}

void SwitchChannel(int channel)
{
	SetCurrentV(channel);
	SetCS(channel);
}

void SwitchGain(int channel) // Use this to change active DAC
{
	if (channel == Z_CH_H)
	{
		currentV.chGain = Z_CH_H;
		SwitchChannel(channel);
	}
	else if (channel == Z_CH_L)
	{
		currentV.chGain = Z_CH_L;
		SwitchChannel(channel);
	}
}

void SetHG()
{
	SwitchGain(Z_CH_H);
	GAIN = ON;
}

void SetLG()
{
	SwitchGain(Z_CH_L);
	GAIN = OFF;
}

void SwapV()
{
	float pullback = 0.0;
	Hold(ON);
	if(currentV.vLG < 0)
	{
		SetLG();
		WriteSPI(VoltstoDAC(0.0)); // Set LG to zero
		currentV.vLG = 0.0;
	}
	else
	{
		SetHG();
		currentV.vHG += 1.0;
		WriteSPI(VoltstoDAC(currentV.vHG));
		delay(Tpid);
		SetLG();
		WriteSPI(VoltstoDAC(0.0)); // Set LG to zero
		currentV.vLG = 0.0;
	}
	SetHG();
	Hold(OFF); // Start the PID loop so that the HG can equilibrate
}

void ChangeZLG()
{
	float incr = 0.0;
	
	StopServo(); // Ensure that the servo is in hold
	
	SetCS(Z_CH_L);
	incr = ReceiveFloat();
	currentV.vLG += incr;
	WriteSPI(VoltstoDAC(currentV.vLG));
}

float ADCtoVolts(long adcval)
{
	return (float)(adcval*ADCRes);	
}

unsigned long VoltstoDAC(float volts)
{
	return (unsigned long)(volts*DACRes+halfDACRes+0.5);
}

float PID(float input,float logset)
{
	lsterr = zerr; // save the last error value
	lstz = zvolts; // save the last output for updating the PID
	pv_2 = pv_1;
	pv_1 = pv_0;
	pv_0 = logf(input);
	zerr = logf(input)-logset; //Calculate the error signal
	//if (zerr < -errmax) zerr = -errmax; //restrict negative errors to max pos error

	// Compute the output from the PID calculation //
	// Consider changing the Kp term to pv_i's
	//return (Kp*(zerr-lsterr) + Ki*Ti*(zerr) - Kd*Td*(pv_0-2*pv_1+pv_2) + lstz);
	return (-Kp*(pv_0-pv_1) + Ki*Ti*(zerr) - Kd*Td*(pv_0-2*pv_1+pv_2) + lstz);
}

void StopServo()
{
	timer_off();
}

void StartServo()
{
	timer_on();
}

void Hold(bool onoff)
{
	if (onoff == 1)	StopServo();
	else StartServo();
	HOLD = onoff; // set hold variable
}

void Retract(bool onoff)
{
	*pUART0THR = onoff; //add 1 char of data to the buffer
    while(!(*pUART0LSR & UARTTHRE)); //wait until it is sent
	if (onoff == ON){
		Hold(ON); // turn hold on or off
		
		/* Switch to High gain and retract */
		if (GAIN == OFF) SetHG();
		WriteSPI(VoltstoDAC(9.5)); // Write 9.5V to DAC
		zvolts = 9.5; // update the current voltage
		currentV.vHG = zvolts;
		
		/* Switch to Low gain and zero */
		SetLG();
		WriteSPI(VoltstoDAC(0.0)); // Write 0.0V to DAC
		currentV.vLG = 0.0;
		
		/* Switch back to High gain */
		SetHG();
		
		offset = MeasureOffset(); // re-average the offset voltage
	}
	else if (onoff == OFF){
		if(RETRACT == ON)
		{
			SetHG();
			Hold(onoff); // turn hold on or off
		}
	}
	RETRACT = onoff;
}

void TipVolts()
{
	bool STATE = HOLD;
	
	float flt = 0.0;
	flt = ReceiveFloat();

	/* Switch the SPI port to select the tip DAC */
	Hold(ON);
	SwitchChannel(TIP_CH);
	
    WriteSPI(VoltstoDAC(flt)); // write value to DAC
    *currentV.current = flt; // update record of DAC volts
    
    /* Switch back to servo channel */
    SwitchChannel(currentV.chGain);
    if (STATE == OFF) Hold(OFF);
    
}

void SetAlpha()
{
	alpha = ReceiveFloat(); // receive new value from serial
	if(alpha <= 0) alpha = 1e-5; // make sure it is between 0 and 1
	else if (alpha > 1.0) alpha = 1.0;
	printf("%f",alpha);
}
    
void CurrentSetpoint()
{
	float flt = 0.0;
	flt = ReceiveFloat();

	setpoint = flt; // update the setpoint
	lsetpoint = logf(flt); // take the log
	errmax = logf(2.5)-lsetpoint; //redefine max err
	//pvmin = expf(errmax+lsetpoint);
}

void SetGain(int whichgain)
{
	float flt = 0.0;
	flt = ReceiveFloat();

	if (whichgain == 1) Kp = flt; // update the gain
	else if (whichgain == 2) Ki = flt; // update the gain
	else if (whichgain == 3) Kd = flt; // update the gain
}

void Reset()
{
	
    Kp = 5e-4;
    Ki = 5e-4;
    Kd = 1e-4;

    zerr = 0;
    lsterr = 0;
    lstz = 0;
    integral = 0;
    zint = 0;
    val = 0;
    fval = 0;
}

void ReportSettings(float kval,int ktype)
{
    char buf[SIZE];
    snprintf(buf, SIZE, "K%d %.4f\n", ktype, kval);
    int i=SIZE;
	while(!(*pUART0LSR & UARTTHRE)); //make sure UART xmit buffer is ready
	for(i=0; i<SIZE; i++)
	{
    	*pUART0THR = buf[i]; //add 1 char of data to the buffer
    	while(!(*pUART0LSR & UARTTHRE)); //wait until it is sent
	}
}

float ReceiveFloat()
{
	BYTE2FLT var1;
	
	/* Receive 4-bytes of Data From Serial */
	while (!(*pUART0LSR & UARTDR));
	var1.BYTE.BYTE0 = *pUART0RBR;
	while (!(*pUART0LSR & UARTDR));
	var1.BYTE.BYTE1 = *pUART0RBR;
	while (!(*pUART0LSR & UARTDR));
	var1.BYTE.BYTE2 = *pUART0RBR;
	while (!(*pUART0LSR & UARTDR));
	var1.BYTE.BYTE3 = *pUART0RBR;
	
	return var1.flt;
}	

void SendFlt(float sflt)
{
	BYTE2FLT var1;
	var1.flt = sflt;
	
	*pUART0THR = var1.BYTE.BYTE0; //add 1 char of data to the buffer
    while(!(*pUART0LSR & UARTTHRE)); //wait until it is sent
    *pUART0THR = var1.BYTE.BYTE1; //add 1 char of data to the buffer
    while(!(*pUART0LSR & UARTTHRE)); //wait until it is sent
    *pUART0THR = var1.BYTE.BYTE2; //add 1 char of data to the buffer
    while(!(*pUART0LSR & UARTTHRE)); //wait until it is sent
    *pUART0THR = var1.BYTE.BYTE3; //add 1 char of data to the buffer
    while(!(*pUART0LSR & UARTTHRE)); //wait until it is sent
}

void SendZ()
{
	if(currentV.chGain == Z_CH_H) SendFlt(currentV.vAcc*16.0);
	else SendFlt(currentV.vAcc);
}

void SendState()
{
	unsigned short sval = 0;
	
	if (RETRACT == ON) sval = 1;
	if (HOLD == ON) sval ^= (1 << HBIT);
	if (GAIN == ON) sval ^= (1 << GBIT);
	
	*pUART0THR = sval; //add 1 char of data to the buffer
    while(!(*pUART0LSR & UARTTHRE)); //wait until it is sent
    *pUART0THR = (sval >> 8); //add 1 char of data to the buffer
    while(!(*pUART0LSR & UARTTHRE)); //wait until it is sent
}	

void ServiceLGFlag()
{
	if (flagLG == ON)
	{
		flagIndex = flagIndex + 1; // Increment the index
		flagIndex = flagIndex % steplen;
		if (flagIndex == 0) // when flagIndex equals steplen, switch to LG
		{
			Hold(ON); // Put servo in hold
			flagLG = OFF; // turn flag off
			SetLG(); // Switch to low gain
			Hold(OFF); // Turn hold off
		}
	}
}

////////// Interrupt Loop ////////////

//Timer interrupt service routine
void timer_isr (int sig)
{
	//ServiceLGFlag();
	// Read the DAC //
	int i =0;
	fval = 0;
	for(i=0;i<ireads;i++)
	{
		fval += ADCtoVolts(ReadSPI());
	}
	fval = -1 * fval * powf((float)ireads,-1);
	
	//val = -1*ReadSPI(); // multiply by -1 to correct sign error
	
	// Convert Integer to Float //
	//fval = fval - offset; //convert int to flt, subtract offset
	if (fval < -2.5) fval = -2.5;
	else if (fval > 2.5) fval = 2.5;
	currentV.vI = fval; // Update the current
	
	if (fval < 0) fval *= -1; //take the abs()
	if (fval < 0.001) fval = 0.001; //make sure the log doesnt blow up

	// Calculate the PID response //
	zvolts = PID(fval,lsetpoint);
	if (zvolts > zmax) zvolts = zmax;
	if (zvolts < -zmax) zvolts = -zmax;
	
	// Convert the setpoint back to data //
	zint = VoltstoDAC(zvolts);
		
	// Write to DAC //
	WriteSPI(zint);	
	*currentV.current = zvolts; // update record
	currentV.vAcc = (1-alpha)*currentV.vAcc + alpha*zvolts;
}

/*void timer_isr (int sig)
{
	float outval=0.0;
	// Read the DAC //
	val = -1*ReadSPI(); // multiply by -1 to correct sign error
	
	zerone = zerone + 1;
	zerone = zerone % 50;
	
	if (zerone < 25) outval = 0.0;
	else outval = 0.1;
	
	// Write to DAC //
	WriteSPI(VoltstoDAC(outval));	
}*/

void timer_step (int sig)
{
	index = index + 1;
	index = index % steplen;
	findex = (float)(0.0 + index);
	// Read the DAC //
	val = -1*ReadSPI(); // multiply by -1 to correct sign error
	
	// Convert Integer to Float //
	fval = ADCtoVolts(val) - offset; //convert int to flt, subtract offset
	if (fval < -2.5) fval = -2.5;
	else if (fval > 2.5) fval = 2.5;
	currentV.vI = fval; // Update the current
	
	if (fval < 0) fval *= -1; //take the abs()
	if (fval < 0.001) fval = 0.001; //make sure the log doesnt blow up

	// Calculate the PID response //
	if (2*index < steplen) zvolts = PID(fval,lsetpoint);
	else zvolts = PID(fval,lsetstepmax);
	if (zvolts > zmax) zvolts = zmax;
	if (zvolts < -zmax) zvolts = -zmax;
	
	// Convert the setpoint back to data //
	zint = VoltstoDAC(zvolts);
		
	// Write to DAC //
	WriteSPI(zint);	
	*currentV.current = zvolts; // update record
}

void StepResponse(bool onoff)
{
	if (onoff == ON){
		StopServo();
		interrupts (SIG_TMZ0, timer_step); /* enable high priority timer interrupt */
    	timer_set(Tpid,Tpid);              /* set tperiod and tcount of the timer */
    	StartServo();
	}
	else if (onoff == OFF){
		StopServo();
		interrupts (SIG_TMZ0, timer_isr); /* enable high priority timer interrupt */
    	timer_set(Tpid,Tpid);              /* set tperiod and tcount of the timer */
    	//errmax = logf(2.5)-lsetpoint; //redefine max err
    	StartServo();
	}
}

/////////////////////////
/*   Enter Main Loop   */
/////////////////////////

void main()
{
	volatile char tempval;
	
	initPLL_SDRAM();  // initialize the PLL
	SetupADC();
	SetupDAC();
	SetupUART();
	SetupGPIO();
	
	currentV.vHG = 0.0;
	currentV.vLG = 0.0;
	currentV.vTip = 0.0;
	currentV.vI = 0.0;
	currentV.vAcc = 0.0;
	currentV.current = &currentV.vHG; // Set pointer to current as high gain
	currentV.chGain = Z_CH_H; // Set the gain channel as high gain
		
	interrupts (SIG_TMZ0, timer_isr); /* enable high priority timer interrupt */
    timer_set(Tpid,Tpid);              /* set tperiod and tcount of the timer */
    lsetpoint = logf(setpoint);
    lsetstepmax = logf(setstepmax);
    errmax = logf(2.5)-lsetpoint; //redefine max err
    Ts = Tpid*powf(331.776e6,-1);
    Ti = 0.5*Ts*powf(DeadT,-1); // sample time divided by dead time
    Td = 0.5*DeadT*powf(Ts,-1);

	Retract(ON);
	//WriteSPI(VoltstoDAC(9.5));
	for(;;)
	{
		if (*pUART0LSR & UARTDR)  // Check the line status register for data
		{
			tempval = *pUART0RBR; // read the data from the register
			if (tempval == 0x31) SetGain(1);			//1
			else if (tempval == 0x32) SetGain(2);		//2
			else if (tempval == 0x33) SetGain(3);		//3
			else if (tempval == 0x48) Hold(ON);     //H
			else if (tempval == 0x68) Hold(OFF);    //h
			else if (tempval == 0x52) Retract(ON);  //R
			else if (tempval == 0x72) Retract(OFF); //r
			else if (tempval == 0x6B) Reset();      //k
			else if (tempval == 0x56) TipVolts();   //V
			else if (tempval == 0x76) SendFlt(currentV.vTip); //v
			//else if (tempval == 0x5A) SendFlt(zvolts);//Z
			//else if (tempval == 0x5A) SendFlt(currentV.vAcc);//Z
			else if (tempval == 0x5A) SendZ();//Z
			else if (tempval == 0x44) SendFlt(currentV.vHG);//D
			else if (tempval == 0x64) SendFlt(currentV.vLG);//d
			else if (tempval == 0x69) CurrentSetpoint();   //i
			else if (tempval == 0x49) SendFlt(currentV.vI);   //I
			else if (tempval == 0x4A) SendFlt(offset);   //J
			else if (tempval == 0x53) StepResponse(ON);  //S
			else if (tempval == 0x73) StepResponse(OFF); //s
			else if (tempval == 0x42) SendState(); //B
			else if (tempval == 0x67) SetLG(); //g
			else if (tempval == 0x47) SetHG(); //G
			else if (tempval == 0x62) SwapV(); //b
			else if (tempval == 0x41) IVSetup(); //A
			else if (tempval == 0x61) DoIV(currentV.vTip,currentV.chGain); //a
			else if (tempval == 0x45) ResumeTunneling(); //E
			else if (tempval == 0x63) SendIV(); //c
			else if (tempval == 0x43) ChangeZLG(); //C
			else if (tempval == 0x65) SetAlpha(); //e
		}
	}
}
